#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <QFileDialog>

#include "history.h"
#include "robotaction.h"
#include "actionfileparser.h"
#include "actionfilewriter.h"
#include "nullglcontroller.h"

#include "GUI/Troy/troyframe.h"
#include "Core/Robot/nullbot.h"

#ifdef KINECT
#include "GUI/kinect/kinectkeyframedialog.h"
//#include "GUI/kinect/kinectsnapshotrecorderdialog.h"
#include "GUI/kinect/kinectstreamrecorderdialog.h"
#endif

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
#ifdef TROY_INTERFACE
    troy(&troy::Troy::getSingleton()),
#endif
#ifdef KINECT
    _kinect(new kinect::Kinect(this)),
    _skeleton(NULL),
    _inCalibration(false),
#endif
#ifdef WIIMOTE
    _wiimote(new qWiimote(this)),
    _wiimoteDialog(new WiimoteStatusDialog(_wiimote, this)),
#endif
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    ui->actionRedo->setDisabled(true);
    ui->actionUndo->setDisabled(true);
    ui->actionSave->setDisabled(true);
    ui->actionSave_As->setDisabled(true);
    ui->playPauseBtn->setDisabled(true);
    ui->stopBtn->setDisabled(true);
    ui->nextBtn->setDisabled(true);
    ui->prevBtn->setDisabled(true);
    ui->timeline->setDisabled(true);

    _robot = new Troy();
    _controller = new TroyGLController();
    ui->glDisplay->loadController(_controller);
    ui->glDisplay->loadModel(_robot);
    ui->sliderView->loadModel(_robot);

    _newActionDialog = new NewActionDialog(_robot, this);

    _curAction = NULL;

    mpRobotController = new CRobotController(this);
    mpCtrlDialog = new ctrlDialog(this, mpRobotController);

    connect(ui->glDisplay, SIGNAL(glMouseMove(double, double, double)),
            this, SLOT(onGLMouseMove(double, double, double)));
    connect(ui->glDisplay, SIGNAL(componentSelected(const uint*)), this, SLOT(onComponentSelected(const uint*)));
    connect(ui->glDisplay, SIGNAL(selectionCleared()), this, SLOT(onSelectionCleared()));
    //connect(ui->glDisplay, SIGNAL(editFinished()), this, SLOT(onGLEditFinished()));

    connect(ui->actionNew, SIGNAL(triggered()), this, SLOT(newAction()));
    connect(ui->actionOpen, SIGNAL(triggered()), this, SLOT(onOpenFile()));
    connect(ui->actionSave, SIGNAL(triggered()), this, SLOT(onSaveFile()));
    connect(ui->actionSave_As, SIGNAL(triggered()), this, SLOT(onSaveAs()));
    connect(ui->actionShow, SIGNAL(triggered()), this, SLOT(showCtrlDialog()));


    connect(ui->actionUndo, SIGNAL(triggered()), History::instance(), SLOT(undo()));
    connect(ui->actionRedo, SIGNAL(triggered()), History::instance(), SLOT(redo()));
    connect(History::instance(), SIGNAL(status(bool,bool,QString,QString)), this, SLOT(historyStatus(bool,bool,QString,QString)));

    connect(ui->mirrorLRButton, SIGNAL(clicked()), this, SLOT(onMirrorLRButtonClicked()));
    connect(ui->mirrorRLButton, SIGNAL(clicked()), this, SLOT(onMirrorRLButtonClicked()));

    connect(ui->ikLeftButton, SIGNAL(clicked()), this, SLOT(onIKLeftButtonClicked()));
    connect(ui->ikRightButton, SIGNAL(clicked()), this, SLOT(onIKRightButtonClicked()));

    connect(ui->playPauseBtn, SIGNAL(clicked()), this, SLOT(togglePlayPause()));

    connect(ui->timeline, SIGNAL(needAction()), this, SLOT(newAction()));
    connect(ui->timeline, SIGNAL(editingKeyFrame(int)), this, SLOT(onEditingKeyFrame(int)));
    connect(ui->timeline, SIGNAL(editingSound(int)), this, SLOT(onEditingSound(int)));
    connect(ui->timeline, SIGNAL(editingFinished()), this, SLOT(onTimelineEditFinish()));
    connect(this, SIGNAL(setActionTimeLine(int)), ui->timeline, SLOT(setTime(int)));

#ifdef TROY_INTERFACE
    connect(_robot, SIGNAL(robotChanged()), this, SLOT(updateTroy()));
    //troy::RobotPose pose;
    //pose.setJoint(0, 10);
    qDebug() << "Connecting COM1:" << troy->connect("COM1");
    //troy->showPose(pose);
    //qDebug() << "Connecting COM2:" << troy->connect("COM2");
    //troy->showPose(pose);
    troyTimer.start();
#endif

#ifdef KINECT
    QMenu* kinectMenu = ui->menuBar->addMenu("Kinect");
    _initializeKinectAction = kinectMenu->addAction("Initialize Kinect", this, SLOT(initializeKinect()));
    _calibrateKinectAction = kinectMenu->addAction("Calibrate", this, SLOT(calibrateKinect()));
    kinectMenu->addSeparator();
    _captureKinectKeyFrameAction = kinectMenu->addAction("Capture Frame", this, SLOT(captureKinectKeyFrame()));
    //_kinectSnapshotRecorderAction = kinectMenu->addAction("Snapshot Record", this, SLOT(kinectSnapshotRecord()));
    _kinectStreamRecorderAction = kinectMenu->addAction("Record", this, SLOT(kinectStreamRecord()));

    _kinect->setLoggingLevel(Message::Debug);
    connect(_kinect, SIGNAL(message(QString,Message::Priority)), this, SLOT(receiveMessage(QString,Message::Priority)));

    if (!initializeKinect()) {
        qDebug() << "Kinect support is enabled, but no Kinect was detected, so it was not included this time.";
        qDebug() << "If you would like to use the Kinect, connect the Kinect and then initialize the Kinect from the Kinect menu.";
    }
#endif
#ifdef WIIMOTE
    QMenu* wiimoteMenu = ui->menuBar->addMenu("Wiimote");
    _connectWiimoteAction = wiimoteMenu->addAction("Connect Wiimote", this, SLOT(connectWiimote()));
    _disconnectWiimoteAction = wiimoteMenu->addAction("Disconnect Wiimote", this, SLOT(disconnectWiimote()));
    wiimoteMenu->addSeparator();
    _showWiimoteStatusDialogAction = wiimoteMenu->addAction("Show Wiimote Status", this, SLOT(showWiimoteStatusDialog()));

    connect(_wiimote, SIGNAL(connected()), this, SLOT(connectWiimote()));
    connect(_wiimote, SIGNAL(disconnected()), this, SLOT(disconnectWiimote()));

    // Initialize association
    setWiimote(_wiimoteDialog);

    if (!connectWiimote()) {
        qDebug() << "Wiimote support is enabled, but no Wiimote was connected.";
        qDebug() << "If you have connected a Wiimote, select Connect Wiimote from the Wiimote menu";
    }
#endif
}

MainWindow::~MainWindow()
{
    delete ui;
}

#ifdef WIIMOTE
void MainWindow::associateWiimote(WiimoteStatusDialog *dialog) {
    if (hasWiimoteDialog()) {
        dialog->setCommand(qWiimote::A, "Play/Pause", this, SLOT(togglePlayPause()));
        if (_curAction != NULL) {
            getWiimoteDialog()->setCommand(qWiimote::B, "Stop", _curAction, SLOT(stop()));
        }
#ifdef KINECT
        dialog->setCommand(qWiimote::Down, "Capture Keyframe", this, SLOT(captureKinectKeyFrame()));
        //dialog->setCommand(qWiimote::Left, "Snapshot Record", this, SLOT(kinectSnapshotRecord()));
        dialog->setCommand(qWiimote::Right, "Stream Record", this, SLOT(kinectStreamRecord()));
#endif
        dialog->setCommand(qWiimote::Minus, "Previous Keyframe", this, SLOT(prevKeyFrame()));
        dialog->setCommand(qWiimote::Plus, "Next Keyframe", this, SLOT(nextKeyFrame()));
    }
}
#endif

void MainWindow::historyStatus(bool undo, bool redo, QString undoTip, QString redoTip){
    ui->actionUndo->setEnabled(undo);
    if(undoTip.isEmpty())
        ui->actionUndo->setText(tr("Undo"));
    else
        ui->actionUndo->setText(undoTip);

    ui->actionRedo->setEnabled(redo);
    if(redoTip.isEmpty())
        ui->actionRedo->setText(tr("Redo"));
    else
        ui->actionRedo->setText(redoTip);
}

void MainWindow::onOpenFile() {
    QString filename = QFileDialog::getOpenFileName(this, tr("Select Action File"), "", tr("TiLAR action files (*.txt *.taf)"));
    if(!filename.isEmpty()) {
        // Some tests for the action swapping abilities of the RobotAction class
        //RobotAction* action = ActionFileParser::parse(filename, new NullBot());
        //RobotAction* action = ActionFileParser::parse(filename, _robot);
        //action->setRobot(_robot);
        //loadAction(action);
        loadAction(ActionFileParser::parse(filename, _robot));
        _curFileName = filename;
    }
}

void MainWindow::newAction() {
    RobotAction* newAction;
    if(sender() == ui->timeline) {
        //If the call for a new action comes from the edit key frame dialog,
        //it is because we don't have an action created and we need to
        //force one to be created.  Even pressing "Cancel" will create a
        //default action.
        qDebug() << "Timeline needs RobotAction";
        newAction = _newActionDialog->exec(true);
    }
    else {
        //Otherwise, let them cancel the dialog without it doing anything
        newAction = _newActionDialog->exec(false);
    }

    loadAction(newAction);
}

void MainWindow::createDefaultAction() {
    RobotAction* newAction = new RobotAction(_robot);
    newAction->setName(DEFAULT_NAME);
    newAction->setMotionDuration(DEFAULT_DURATION);
    newAction->setSoundTrackDuration(DEFAULT_DURATION);
    loadAction(newAction);
}

/*
void MainWindow::insertKeyFrame(MotionKeyFrame keyFrame) {
    if (_curAction == NULL) {
        createDefaultAction();
    }
    ActionCommandGroup* group = new ActionCommandGroup(_curAction);
    foreach (JointValue joint, keyFrame.jointValues) {
        //InsertKeyFrameCommand* insertCommand = new InsertKeyFrameCommand(_curAction, keyFrame.timestamp, joint.first);
        //group->append(insertCommand);
        SetKeyFrameCommand* setCommand = new SetKeyFrameCommand(_curAction, keyFrame.timestamp, joint.first, joint.second);
        group->append(setCommand);
    }
    group->setHistoryTip("Insert Key Frame");
    group->execute();
    History::instance()->push(group);
}
*/

void MainWindow::append(RobotAction *action) {
    if (_curAction == NULL) {
        loadAction(action);
    } else {
        ActionCommandGroup* group = new ActionCommandGroup(_curAction);
        int dur = ui->timeline->currentTime(); // _curAction->duration();
        foreach (MotionKeyFrame keyFrame, action->keyFrames()) {
            foreach (JointValue joint, keyFrame.jointValues) {
                //InsertKeyFrameCommand* insertCommand = new InsertKeyFrameCommand(_curAction, keyFrame.timestamp, joint.first);
                //group->append(insertCommand);
                SetKeyFrameCommand* setCommand = new SetKeyFrameCommand(_curAction, dur+keyFrame.timestamp, joint.first, joint.second);
                group->append(setCommand);
            }
        }
        group->setHistoryTip("Insert Key Frame");
        group->execute();
        History::instance()->push(group);
    }
}

void MainWindow::playAction(QString actionFileName)
{
    // TODO: should stop previous action before load new one
    emit stopAnimation();
    resetTimeLine();

    if(!actionFileName.isEmpty()) {
        // Some tests for the action swapping abilities of the RobotAction class
        //RobotAction* action = ActionFileParser::parse(filename, new NullBot());
        //RobotAction* action = ActionFileParser::parse(filename, _robot);
        //action->setRobot(_robot);
        //loadAction(action);
        loadAction(ActionFileParser::parse(actionFileName, _robot));

        if(_curAction->state() == QAbstractAnimation::Stopped)
        {
            executeAction();
        }
    }
}


void MainWindow::executeAction(void)
{
    if(_curAction != NULL){
        if(_curAction->state() == QAbstractAnimation::Paused){
            _curAction->resume();
        }
        else if(_curAction->state() == QAbstractAnimation::Stopped){
            int timelineTime = ui->timeline->currentTime();

            //If we are stopped, but the timeline has a certain time
            //(for example, we haven't ever played the action, but
            //the timeline has been scrubbed).
            if(timelineTime < _curAction->duration()){
                //kill signals from the action so that there isn't
                //a jerk in the GUI
                _curAction->blockSignals(true);
            }

            _curAction->start();

            if(timelineTime < _curAction->duration()){
                //Start playback at the time of the timeline
                _curAction->setCurrentTime(timelineTime);

                //Unblock signals so that the timeline will update
                //appropriately again.
                _curAction->blockSignals(false);
            }
        }
        else if(_curAction->state() == QAbstractAnimation::Running){
            _curAction->pause();
        }
    }

}

void MainWindow::loadAction(RobotAction* action){
    if(_curAction != NULL) {
        _curAction->deleteLater();
        _curAction = NULL;
    }

    _curAction = action;
    if(_curAction != NULL){
        _curAction->setRobot(_robot);
        ui->actionSave->setEnabled(true);
        ui->actionSave_As->setEnabled(true);
        ui->playPauseBtn->setEnabled(true);
        ui->stopBtn->setEnabled(true);
        ui->nextBtn->setEnabled(true);
        ui->prevBtn->setEnabled(true);
        ui->timeline->setEnabled(true);

        connect(_curAction, SIGNAL(stateChanged(QAbstractAnimation::State,QAbstractAnimation::State)),
                this, SLOT(onActionStateChanged(QAbstractAnimation::State,QAbstractAnimation::State)));
        connect(_curAction, SIGNAL(keyFrame(int)), this, SLOT(onKeyFrame(int)));
        connect(_curAction, SIGNAL(soundPlayed(int,QString)), this, SLOT(onSoundPlayed(int,QString)));
        connect(_curAction, SIGNAL(soundFinished(int,QString)), this, SLOT(onSoundFinished(int,QString)));

        connect(ui->nextBtn, SIGNAL(clicked()), _curAction, SLOT(nextKeyFrame()));
        connect(ui->prevBtn, SIGNAL(clicked()), _curAction, SLOT(prevKeyFrame()));
        connect(ui->stopBtn, SIGNAL(clicked()), _curAction, SLOT(stop()));

        connect(this, SIGNAL(stopAnimation()), _curAction, SLOT(stop()));
#ifdef WIIMOTE
        if (getWiimoteDialog() != NULL) {
            getWiimoteDialog()->setCommand(qWiimote::B, "Stop", _curAction, SLOT(stop()));
        }
#endif

        ui->timeline->loadAction(_curAction);

        QLog::log("New Action Loaded by Main Timeline");

        //Test the action file writer
    } else {
        qDebug() << "Loading null action...";
    }
}



void MainWindow::onSaveFile(){
    ActionFileWriter::write("C:\\Users\\tmajor_2\\Documents\\TiLAR\\TAI-Repository\\GLInterface\\test.txt", _curAction);
}

void MainWindow::onSaveAs(){

}

void MainWindow::onActionStateChanged(QAbstractAnimation::State newState, QAbstractAnimation::State prevState){
    if(newState == QAbstractAnimation::Paused){
        ui->playPauseBtn->setText("Play");
        appendEventInfo("ActionStateChanged: Paused");
    }
    else if(newState == QAbstractAnimation::Stopped){
        ui->playPauseBtn->setText("Play");
        appendEventInfo("ActionStateChanged: Stopped");
    }
    else if(newState == QAbstractAnimation::Running){
        ui->playPauseBtn->setText("Pause");
        appendEventInfo("ActionStateChanged: Running");
    }
}

void MainWindow::onKeyFrame(int msec){
    appendEventInfo("Keyframe: " + QString::number(msec));
}

void MainWindow::onSoundPlayed(int msec, QString name){
    appendEventInfo("Sound played: " + QString::number(msec));
}

void MainWindow::nextKeyFrame() {
    if (_curAction != NULL) {
        _curAction->nextKeyFrame();
    }
}

void MainWindow::prevKeyFrame() {
    if (_curAction != NULL) {
        _curAction->prevKeyFrame();
    }
}

void MainWindow::onSoundFinished(int msec, QString name){
    appendEventInfo("Sound finished: " + QString::number(msec));
}


void MainWindow::onGLMouseMove(double x, double y, double z){
    //Display the mouse coordinates from the OpenGL display
    QString cursorInfo = "";
    QTextStream cursorStream(&cursorInfo);
    cursorStream << x << "," << y << "," << z;

    ui->cursorInfo->setText(cursorInfo);
}


void MainWindow::onComponentSelected(const unsigned int* componentNames){
    int i;
    QString infoString;
    QTextStream infoStream(&infoString);
    const RobotComponent* robotComponent = _robot->getComponent(componentNames);
    QList<int> enabledJointIDs, disabledJointIDs;

    if(robotComponent != NULL){
        Dimension3D dimensions = robotComponent->dimensions();
        const unsigned int* names = robotComponent->names();

        infoStream << "Names:          [";
        for(i = 0; i < MAX_NAMES; i++){
            infoStream << names[i];
            if(i != (MAX_NAMES - 1))
                infoStream << ", ";
            else
                infoStream << "]\n";
        }
        infoStream << "Description:  \"" << robotComponent->description() << "\"\n";


        if(robotComponent->type() == SEGMENT){
            infoStream << "Type:             SEGMENT\n";
            infoStream << "Dimensions:   (" << dimensions.x << ", " << dimensions.y << ", " << dimensions.z << ")\n";

            QVector3D endPoint = ((RobotSegment*)robotComponent)->endPoint();
            infoStream << "End point:      (" << endPoint.x() << ", " << endPoint.y() << ", " << endPoint.z() << ")\n";
        }
        else if(robotComponent->type() == JOINT_GROUP){
            infoStream << "Type:             JOINT_GROUP\n";
            infoStream << "Dimensions:   (" << dimensions.x << ", " << dimensions.y << ", " << dimensions.z << ")\n";

            infoStream << "Joints:\n";
            QList<int> jointIDs = ((RobotJointGroup*)robotComponent)->jointIDs();
            for(i = 0; i < jointIDs.length(); i++){
                infoStream << " " << jointIDs.at(i) << ":  \"";
                infoStream << ((RobotJointGroup*)robotComponent)->jointDescription(jointIDs.at(i)) << "\"\n";
            }
        }

        setSelectionInfo(infoString);

        if(robotComponent->type() == SEGMENT){
            RobotJointGroup* parent = ((RobotJointGroup*)robotComponent->parent());
            if(parent != NULL){
                enabledJointIDs = parent->jointIDs();
            }
        }
        else if(robotComponent->type() == JOINT_GROUP){
            enabledJointIDs = ((RobotJointGroup*)robotComponent)->jointIDs();
        }

        disabledJointIDs = _robot->jointIDs();
        for(i = 0; i < enabledJointIDs.length(); i++){
            disabledJointIDs.removeAll(enabledJointIDs[i]);
        }
        ui->sliderView->enable(enabledJointIDs);
        ui->sliderView->disable(disabledJointIDs);


    }
}

void MainWindow::onSelectionCleared(){
    ui->sliderView->enableAll();
    ui->selectionInfo->setText("");
}

void MainWindow::onMirrorLRButtonClicked(){
    TroyController controller((Troy*)_robot);
    controller.mirror(TroyController::LEFT_TO_RIGHT);
}

void MainWindow::onMirrorRLButtonClicked(){
    TroyController controller((Troy*)_robot);
    controller.mirror(TroyController::RIGHT_TO_LEFT);
}

void MainWindow::onIKLeftButtonClicked(){
    int calculations;
    NameGroup segmentNames;
    segmentNames[0] = Troy::T_LEFT_FOREARM;
    segmentNames[1] = 0;
    segmentNames[2] = 0;
    segmentNames[3] = 0;
    segmentNames[4] = 0;

    QList<int> jointIDs;
    if(ui->flexionCheckBox->isChecked())
        jointIDs.append(0);
    if(ui->abductionCheckBox->isChecked())
        jointIDs.append(1);
    if(ui->rotationCheckBox->isChecked())
        jointIDs.append(8);
    if(ui->elbowCheckBox->isChecked())
        jointIDs.append(9);

    QVector3D goal;
    goal.setX(ui->xGoalSpinBox->value());
    goal.setY(ui->yGoalSpinBox->value());
    goal.setZ(ui->zGoalSpinBox->value());

    calculations = _controller->inverseKinematics(segmentNames, goal, jointIDs);
    ui->calculationCount->setText(QString::number(calculations));
}

void MainWindow::onIKRightButtonClicked(){
    int calculations;
    NameGroup segmentNames;
    segmentNames[0] = Troy::T_RIGHT_FOREARM;
    segmentNames[1] = 0;
    segmentNames[2] = 0;
    segmentNames[3] = 0;
    segmentNames[4] = 0;

    QList<int> jointIDs;
    if(ui->flexionCheckBox->isChecked())
        jointIDs.append(16);
    if(ui->abductionCheckBox->isChecked())
        jointIDs.append(17);
    if(ui->rotationCheckBox->isChecked())
        jointIDs.append(24);
    if(ui->elbowCheckBox->isChecked())
        jointIDs.append(25);

    QVector3D goal;
    goal.setX(ui->xGoalSpinBox->value());
    goal.setY(ui->yGoalSpinBox->value());
    goal.setZ(ui->zGoalSpinBox->value());

    calculations = _controller->inverseKinematics(segmentNames, goal, jointIDs);
    ui->calculationCount->setText(QString::number(calculations));
}


void MainWindow::togglePlayPause(){
    if(_curAction != NULL){
        if(_curAction->state() == QAbstractAnimation::Paused){
            _curAction->resume();
        }
        else if(_curAction->state() == QAbstractAnimation::Stopped){
            int timelineTime = ui->timeline->currentTime();

            //If we are stopped, but the timeline has a certain time
            //(for example, we haven't ever played the action, but
            //the timeline has been scrubbed).
            if(timelineTime < _curAction->duration()){
                //kill signals from the action so that there isn't
                //a jerk in the GUI
                _curAction->blockSignals(true);
            }

            _curAction->start();

            if(timelineTime < _curAction->duration()){
                //Start playback at the time of the timeline
                _curAction->setCurrentTime(timelineTime);

                //Unblock signals so that the timeline will update
                //appropriately again.
                _curAction->blockSignals(false);
            }
        }
        else if(_curAction->state() == QAbstractAnimation::Running){
            _curAction->pause();
        }
    }
}


void MainWindow::appendEventInfo(QString info){
    ui->eventInfo->append(info);
}

void MainWindow::setSelectionInfo(QString info){
    ui->selectionInfo->setText(info);
}

void MainWindow::allowGLEditing(bool allowed){
    ui->glDisplay->allowEditing(allowed);
}

void MainWindow::onEditingKeyFrame(int timestamp){
    allowGLEditing(true);
    //Somehow highlight the openGL display to indicate it
    //is editable
    ui->glDisplay->renderText(0,0,"TEST");
}

void MainWindow::onEditingSound(int timestamp){

}

void MainWindow::onTimelineEditFinish(){
    allowGLEditing(false);
}


void MainWindow::onGLEditFinished(){
    qDebug() << "Edit finished";
}

void MainWindow::moveEvent(QMoveEvent *event) {
#ifdef WIIMOTE
    int deltaX = event->pos().x() - event->oldPos().x();
    int deltaY = event->pos().y() - event->oldPos().y();
    if (_wiimoteDialog != NULL) {
        _wiimoteDialog->move(_wiimoteDialog->x() + deltaX, _wiimoteDialog->y() + deltaY);
    }
#endif
}

#ifdef KINECT
void MainWindow::onDialogClose(int result) {
    if (sender() != NULL) {
        sender()->deleteLater();
    }
}

bool MainWindow::initializeKinect() {
    if (!_kinect->isValid()) {
        _kinect->reconnect();
        if (_kinect->isValid()) {
            //_kinect->setLoggingLevel(Message::Debug);
            //connect(_kinect, SIGNAL(message(QString,Message::Priority)), this, SLOT(emitMessage(QString,Message::Priority)));
            _skeleton = _kinect->addComponent<kinect::KinectSkeleton>();

            _initializeKinectAction->setDisabled(true);
            _calibrateKinectAction->setDisabled(false);
            _captureKinectKeyFrameAction->setDisabled(false);
            //_kinectSnapshotRecorderAction->setDisabled(false);
            _kinectStreamRecorderAction->setDisabled(false);

            return true;
        } else {
            _skeleton = NULL;
            _initializeKinectAction->setDisabled(false);
            _calibrateKinectAction->setDisabled(true);
            _captureKinectKeyFrameAction->setDisabled(true);
            //_kinectSnapshotRecorderAction->setDisabled(true);
            _kinectStreamRecorderAction->setDisabled(true);
            return false;
        }
    } else {
        _initializeKinectAction->setDisabled(true);
        _calibrateKinectAction->setDisabled(false);
        _captureKinectKeyFrameAction->setDisabled(false);
        //_kinectSnapshotRecorderAction->setDisabled(false);
        _kinectStreamRecorderAction->setDisabled(false);
        return true;
    }
}

void MainWindow::captureKinectKeyFrame() {
    QLog::log("Capturing Kinect Keyframe");
    KinectKeyFrameDialog* dialog = new KinectKeyFrameDialog(_kinect, this);
#ifdef WIIMOTE
    QWidget* old = _wiimoteDialog->associate(dialog);
#endif
    if (dialog->exec()) {
        QLog::log("Kinect Keyframe Captured");
        //MotionKeyFrame keyFrame = dialog->getKeyFrame();
        if (_curAction == NULL) {
            createDefaultAction();
        }
        //keyFrame.timestamp = qBound(100, _curAction->currentTime(), _curAction->duration()-100);
        //insertKeyFrame(keyFrame);
    } else {
        QLog::log("Kinect Keyframe Cancelled");
    }
#ifdef WIIMOTE
    _wiimoteDialog->associate(old);
#endif
    dialog->deleteLater();
}

/*
void MainWindow::kinectSnapshotRecord() {
    QLog::log("Recording Kinect by Snapshots");
    KinectSnapshotRecorderDialog* dialog = new KinectSnapshotRecorderDialog(_kinect, this);
#ifdef WIIMOTE
    QWidget* old = _wiimoteDialog->associate(dialog);
#endif
    if (dialog->exec()) {
        QLog::log("Kinect Snapshot Recorded");
        // translate and insert the recorded action
        RobotAction* action = dialog->getRecordedAction();
        if (action != NULL) {
            append(action);
        }
    } else {
        QLog::log("Kinect Snapshot Cancelled");
    }
#ifdef WIIMOTE
    _wiimoteDialog->associate(old);
#endif
    dialog->deleteLater();
}
*/

void MainWindow::kinectStreamRecord() {
    QLog::log("Recording Kinect Raw");
    KinectStreamRecorderDialog* dialog = new KinectStreamRecorderDialog(_kinect, this);
#ifdef WIIMOTE
    QWidget* old = _wiimoteDialog->associate(dialog);
#endif
    if (dialog->exec()) {
        QLog::log("Kinect Raw Recorded");
        // translate and insert the recorded action
        RobotAction* action = dialog->getRecordedAction();
        if (action != NULL) {
            append(action);
        }
    } else {
        QLog::log("Kinect Raw Cancelled");
    }
#ifdef WIIMOTE
    _wiimoteDialog->associate(old);
#endif
    dialog->deleteLater();
}

void MainWindow::calibrateKinect() {
    if (!_inCalibration) {
        _inCalibration = true;
        QLog::log("Calibrating Kinect");
        _skeleton->reset();
#ifdef WIIMOTE
        if (KinectDialog::calibrate(_kinect, _wiimoteDialog, this)) {
#else
        if (KinectDialog::calibrate(_kinect, this)) {
#endif
            QLog::log("Kinect Calibrated");
        } else {
            QLog::log("Kinect Calibration Cancelled");
        }
        _inCalibration = false;
    }
}

void MainWindow::receiveMessage(QString message, Message::Priority priority) {
    //qDebug() << message;
    ui->eventInfo->append("Kinect: "+message);
}

#endif

#ifdef WIIMOTE
bool MainWindow::connectWiimote() {
    if (!_wiimote->isConnected()) {
        _wiimote->reconnect();
    }
    if (_wiimote->isConnected()) {
        _wiimoteDialog->show();
        //associateWiimote(_wiimoteDialog);

        _connectWiimoteAction->setDisabled(true);
        _disconnectWiimoteAction->setDisabled(false);
        _showWiimoteStatusDialogAction->setDisabled(false);
        return true;
    } else {
        _wiimote->disconnect();

        _connectWiimoteAction->setDisabled(false);
        _disconnectWiimoteAction->setDisabled(true);
        _showWiimoteStatusDialogAction->setDisabled(true);
        return false;
    }
}

void MainWindow::disconnectWiimote() {
    //if (_wiimote != NULL) {
        _wiimote->disconnect();

        _connectWiimoteAction->setDisabled(false);
        _disconnectWiimoteAction->setDisabled(true);
        _showWiimoteStatusDialogAction->setDisabled(true);
    //}
}

void MainWindow::showWiimoteStatusDialog() {
    //if (_wiimote != NULL) {
        _wiimoteDialog->show();
    //}
}

#endif

#ifdef TROY_INTERFACE
void MainWindow::updateTroy() {
    //troy::RobotPose pose;
    //qDebug() << "troyTimer elapsed: " << troyTimer.elapsed();
    //if (troyTimer.elapsed() > 1000) {
        foreach (int joint, _robot->jointIDs()) {
            //pose.setJoint(joint, _robot->jointAngle(joint));
            troy->hackJoint(joint, _robot->jointAngle(joint));
        }
        //troyTimer.restart();
    //}
    //troy->showPose(pose);
}
#endif

void MainWindow::showCtrlDialog()
{
    if (mpCtrlDialog)
    {
        mpCtrlDialog->show();
    }

}

void MainWindow::startController(void)
{
    qDebug("Start controller \n");
    if (mpRobotController)
    {
        mpRobotController->start();
    }

}

void MainWindow::stopController(void)
{
    qDebug("Stop controller \n");
    emit stopAnimation();

    if (mpRobotController)
    {
        mpRobotController->stop();
    }
}

void MainWindow::runController(void)
{
    qDebug("Run controller \n");
    if (mpRobotController)
    {
        mpRobotController->run();
    }
}

void MainWindow::pauseController(void)
{
    qDebug("Pause controller \n");
    if (mpRobotController)
    {
        mpRobotController->pause();
    }
}

void MainWindow::resetTimeLine(void)
{
    emit setActionTimeLine(0);
}
